#ifndef __SV2_ELLIPSE_DET__
#define __SV2_ELLIPSE_DET__

#include "sms_core.h"
#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <mutex>
#include <queue>
#include <condition_variable>
#include "ellipse_detector.h"
#define SV2_RAD2DEG 57.2957795

/*
依赖安装
sudo apt install libopencv-dev
sudo apt install libopencv-contrib-dev

*/


namespace sv2 {


class ImgMsg {
  public:
    ImgMsg(cv::Mat img_, nlohmann::json msg_) : img(img_), msg(msg_) {}
    cv::Mat img;
    nlohmann::json msg;
};


class EllipseDetCppNode : public sms::BaseNode
{
public:
  EllipseDetCppNode(
    std::string job_name,
    std::string param_file,
    std::string ip="127.0.0.1",
    int port=9094,
    nlohmann::json kwargs = {}
  ) : sms::BaseNode("EllipseDetCppNode", job_name, param_file, ip, port, kwargs), 
    _image_sub("/" + job_name + "/sensor/image_raw", "std_msgs::Null", std::bind(&EllipseDetCppNode::image_callback, this, std::placeholders::_1)),
    _calib_sub("/" + job_name + "/sensor/calibration_info", "sensor_msgs::CameraCalibration", std::bind(&EllipseDetCppNode::calib_callback, this, std::placeholders::_1)),
    _res_pub("/" + job_name + "/detector/results", "spirecv_msgs::2DTargets"),
    _vis_pub("/" + job_name + "/detector/image_results", "memory_msgs::RawImage")
  {
    // 读取节点参数
    this->_radius_in_meter = this->get_param("radius_in_meter", 0.5);
    this->_pre_processing_gauss_kernel = this->get_param("pre_processing_gauss_kernel", 5);
    this->_pre_processing_gauss_sigma = this->get_param("pre_processing_gauss_sigma", 1.306);
    this->_th_position = this->get_param("th_position", 1.0);
    this->_max_center_distance = this->get_param("max_center_distance", 0.05);
    this->_min_edge_length = this->get_param("min_edge_length", 9);
    this->_min_oriented_rect_side = this->get_param("min_oriented_rect_side", 2.984);
    this->_distance_to_ellipse_contour = this->get_param("distance_to_ellipse_contour", 0.111);
    this->_min_score = this->get_param("min_score", 0.511);
    this->_min_reliability = this->get_param("min_reliability", 0.470);
    this->_ns = this->get_param("ns", 22);
    this->_percent_ne = this->get_param("percent_ne", 0.946);
    this->_T_CNC = this->get_param("T_CNC", 0.121);
    this->_T_TCN_L = this->get_param("T_TCN_L", 0.468);
    this->_T_TCN_P = this->get_param("T_TCN_P", 0.560);
    this->_th_radius = this->get_param("th_radius", 0.202);
    this->params_help();


    this->camera_matrix = cv::Mat::zeros(3, 3, CV_64FC1);
    this->distortion = cv::Mat::zeros(1, 5, CV_64FC1);

    this->_t0 = 0;
    this->_first_show_calib = false;
    this->image_width = 0;
    this->image_height = 0;
    this->fov_x = 0;
    this->fov_y = 0;
    _ed = NULL;

    if (this->_ed == NULL)
    {
      cv::Size szPreProcessingGaussKernel(this->_pre_processing_gauss_kernel, this->_pre_processing_gauss_kernel);
      double fMaxCenterDistance = sqrt(double(1280 * 1280 + 720 * 720)) * this->_max_center_distance;

      _ed = new yaed::EllipseDetector;
      _ed->SetParameters(
        szPreProcessingGaussKernel, 
        this->_pre_processing_gauss_sigma, 
        this->_th_position, 
        fMaxCenterDistance, 
        this->_min_edge_length, 
        this->_min_oriented_rect_side, 
        this->_distance_to_ellipse_contour, 
        this->_min_score, 
        this->_min_reliability, 
        this->_ns, 
        this->_percent_ne, 
        this->_T_CNC, 
        this->_T_TCN_L, 
        this->_T_TCN_P, 
        this->_th_radius
      );
    }
  }
  ~EllipseDetCppNode()
  {
    if (_ed) { delete _ed; _ed = NULL; }
  }

  void run();

  // 图像话题回调函数
  void image_callback(nlohmann::json msg)
  {
    if (!this->_image_queue.empty())
      return;

    cv::Mat img = sms::sms2cvimg(msg);
    ImgMsg imgmsg(img, msg);

    // 放入到阻塞队列中
    {
      std::unique_lock<std::mutex> lock(this->_image_queue_mtx);
      this->_image_queue.push(imgmsg);
    }
  }
  // 相机参数话题回调函数
  void calib_callback(nlohmann::json msg)
  {
    int i(0);
    for (const auto& item : msg["K"]) { this->camera_matrix.at<double>(i++) = item; }

    i = 0;
    for (const auto& item : msg["D"]) { this->distortion.at<double>(i++) = item; }

    this->image_width = msg["width"];
    this->image_height = msg["height"];

    this->fov_x = 2 * atan(this->image_width / 2. / camera_matrix.at<double>(0, 0)) * SV2_RAD2DEG;
    this->fov_y = 2 * atan(this->image_height / 2. / camera_matrix.at<double>(1, 1)) * SV2_RAD2DEG;

    if (!this->_first_show_calib)
    {
      this->_first_show_calib = true;
      logger.info("K: " + msg["K"].dump());
      logger.info("D: " + msg["D"].dump());
      logger.info("ImageSize: (" + std::to_string(this->image_width) + ", " + std::to_string(this->image_height) + ")");
      logger.info("FOV: (" + std::to_string(this->fov_x) + ", " + std::to_string(this->fov_y) + ")");
    }
  }
  

  cv::Mat camera_matrix;
  cv::Mat distortion;
  int image_width;
  int image_height;
  double fov_x;
  double fov_y;

private:
  double _radius_in_meter;
  int _pre_processing_gauss_kernel;
  double _pre_processing_gauss_sigma;
  double _th_position;
  double _max_center_distance;
  int _min_edge_length;
  double _min_oriented_rect_side;
  double _distance_to_ellipse_contour;
  double _min_score;
  double _min_reliability;
  int _ns;
  double _percent_ne;
  double _T_CNC;
  double _T_TCN_L;
  double _T_TCN_P;
  double _th_radius;
  
  yaed::EllipseDetector* _ed;
  bool _first_show_calib;

  // 订阅话题
  sms::Subscriber _image_sub;
  sms::Subscriber _calib_sub;
  // 发布话题
  sms::Publisher _res_pub;
  sms::Publisher _vis_pub;

  std::mutex _image_queue_mtx;
  std::queue<ImgMsg> _image_queue;
  double _t0;

  nlohmann::json _publish_to_sms(std::vector<yaed::Ellipse>& ellsCned, int img_w_, int img_h_);
};


}
#endif
